//
// Created by az on 2020/7/15.
//

#include "wmr_nav/VSTTM_nav.h"

void VSTTM::init(ros::NodeHandle &nh) {
    nh.param("navigation/max_v", Vmax_, 1.0);
    nh.param("navigation/max_w", Wmax_, 90 * D2R);
    nh.param("navigation/max_dv", DVmax_, 0.5);
    nh.param("navigation/max_dw", DWmax_, 180 * D2R);
    nh.param("navigation/execute_period", exe_time_, 0.1);
    nh.param("navigation/prediction_period", pre_time_, 2.0);

    nh.param("navigation/q1", q1_, 1.0);
    nh.param("navigation/q2", q2_, 1.0);
    nh.param("navigation/q3", q3_, 0.5);

    nh.param("robot/radius", robot_radius_, 0.2);
    nh.param("distance_retraj", dis_retraj_, 1.0);

    nh.param<std::string>("record/file_path", path_name_,
                          std::string("/home/nvidia/Data/jg/"));

    //获取当前系统的当前日期/时间
    std::time_t now = std::time(0);
    std::tm *ltm = std::localtime(&now);

    std::ostringstream ostr;
    ostr << 1 + ltm->tm_mon << "_" << ltm->tm_mday << "_" << ltm->tm_hour << "_"
         << ltm->tm_min << "_" << ltm->tm_sec << ".vsttm";

    std::string time_name, file_name;
    time_name = ostr.str();
    file_name = path_name_ + time_name;

    out_file_.open(file_name);

    if (!out_file_) {
        std::cerr << "[wmr nav] open output file error !" << std::endl;
        exit(-1);
    }
}

void VSTTM::setEnvironment(MAP_EDT::Ptr &map_edt, Bernstein::Ptr &bezier) {
    map_edt_ = map_edt;
    bezier_ = bezier;
}

void VSTTM::setRobotState(double time, Eigen::Vector3d &p_3d, double &yaw, double &v, double &w) {
    t_now_ = time;
    p_.reset(new RobState(p_3d, yaw, v, w));

    file_lock_.lock();
    out_file_ << "$p," << double2string(t_now_) << ","
    << double2string(p_->pt[0]) << ","
    << double2string(p_->pt[1]) << ","
    << double2string(yaw) << "," << double2string(v) << "," << double2string(w) << std::endl;
    file_lock_.unlock();
}

void VSTTM::setBTraj(int seg_num, double stime, Eigen::MatrixXd &coeff, Eigen::VectorXd &seg_time) {
    btraj_.seg_num = seg_num;
    btraj_.start_time = stime;
    btraj_.traj_coeff = coeff;
    btraj_.seg_time = seg_time;

    btraj_.duration = 0;
    for (int i = 0; i < seg_num; i++)
        btraj_.duration += seg_time[i];

    auto p0 = bezier_->getPosFromBezier(btraj_.traj_coeff, 0, 0);
    auto p1 = bezier_->getPosFromBezier(btraj_.traj_coeff, 1, 0);
    auto dd = (p1 - p0) * seg_time[0];
    btraj_.init_yaw = atan2(dd[1], dd[0]);
}

void VSTTM::setBspline(vector<NonUniformBspline> traj, double start_t) {
    bspline_traj_.pos_traj = traj[0];
    bspline_traj_.vel_traj = traj[1];
    bspline_traj_.acc_traj = traj[2];
    bspline_traj_.start_time = start_t;
    bspline_traj_.duration = traj[0].getTimeSum();
}

void VSTTM::setStartTime(double start_t){ bspline_traj_.start_time = start_t; }

bool VSTTM::run(double &velocity, double &angular) {

    if (!getPr()) {
        velocity = 0;
        angular = 0;
        return false;
    }

    //计算出可行速度窗口
    calcDynamicWindow(vmin_, vmax_, wmin_, wmax_);

    //计算窗口内的可行速度的预估轨迹和三项系数
    evaluation();
    //无可行路径
    if (vec_eval_.empty()) {
        ROS_WARN("no path to goal !");
        velocity = 0;
        angular = 0;

        return false;
    }
    //系数正规化
    normalize();
    //最终评价
    finalEvaluate();

    velocity = v_output_;
    angular = w_output_;
    auto& _pr_tmp = vec_pr_.front();
    if(_pr_tmp->angular > Wmax_ && _pr_tmp->angular < -Wmax_)
        angular = 0.0;

    recordError();

    return true;
}

void VSTTM::recordError() {
    auto& _pr_tmp = vec_pr_.front();

    auto& pr = _pr_tmp->pt;
    double xe = cos(p_->yaw) * (pr[0] - p_->pt[0]) + sin(p_->yaw) * (pr[1] - p_->pt[1]);
    double ye = -sin(p_->yaw) * (pr[0] - p_->pt[0]) + cos(p_->yaw) * (pr[1] - p_->pt[1]);
    double thetae = _pr_tmp->yaw - p_->yaw;
    while (thetae > M_PI) {
        thetae -= 2 * M_PI;
    }
    while (thetae < -M_PI) {
        thetae += 2 * M_PI;
    }

    file_lock_.lock();
    out_file_ << "$error," << double2string(t_now_) << ","
              << double2string(xe) << ","
              << double2string(ye) << ","
              << double2string(thetae) << std::endl;
    file_lock_.unlock();

}

void VSTTM::calcDynamicWindow(double &vmin, double &vmax, double &wmin, double &wmax) {
    double vd_vmin = p_->velocity - DVmax_ * exe_time_;
    double vd_vmax = p_->velocity + DVmax_ * exe_time_;
    double vd_wmin = p_->angular - DWmax_ * exe_time_;
    double vd_wmax = p_->angular + DWmax_ * exe_time_;

    vmin = min(Vmax_, max(vd_vmin, -Vmax_));
    vmax = min(Vmax_, max(vd_vmax, -Vmax_));
    wmin = min(Wmax_, max(vd_wmin, -Wmax_));
    wmax = min(Wmax_, max(vd_wmax, -Wmax_));
}

bool VSTTM::getPr() {
    vector<RobState::Ptr>().swap(vec_pr_);
    double yaw, v, w;

    /*vsttm test*/
//    double _x0 = -2, _y0 = 4;
//    double _x, _y, _R = 3, _v = 0.8;
//    double _w = _v / _R;

    double t_cur = t_now_ - bspline_traj_.start_time, t_traj;
    Eigen::Vector3d pt, vel, acc;
    for (double tp = 0.0; tp <= pre_time_; tp += exe_time_) {
        t_traj = t_cur + tp;

        if (t_traj >= bspline_traj_.duration - 1e-7) break;
        pt = bspline_traj_.pos_traj.evaluateDeBoorT(t_traj);
        vel = bspline_traj_.vel_traj.evaluateDeBoorT(t_traj);
        acc = bspline_traj_.acc_traj.evaluateDeBoorT(t_traj);

        yaw = atan2(vel[1], vel[0]);
        v = vel.norm();
        w = (acc(1) * vel(0) - acc(0) * vel(1))
            / (pow(vel(0), 2) + pow(vel(1), 2));

        /*
        yaw = atan2(cos(_w * t_traj), -sin(_w * t_traj));
        _x  = _x0 + _R * cos(_w * t_traj);
        _y  = _y0 + _R * sin(_w * t_traj);
        v = _v;
        w = _w;
        pt << _x, _y, 0.01;
         */

        RobState::Ptr state;
        state = std::make_shared<RobState>(pt, yaw, v, w);
        vec_pr_.push_back(state);
    }


    if (!vec_pr_.empty()) {

        auto& pf = vec_pr_.front();
        file_lock_.lock();
        out_file_ << "$pr," << double2string(t_now_) << ","
                  << double2string(pf->pt[0]) << ","
                  << double2string(pf->pt[1]) << ","
                  << double2string(pf->yaw) << ","
                  << double2string(pf->velocity) << ","
                  << double2string(pf->angular) << std::endl;
        file_lock_.unlock();

        return true;
    } else {
        ROS_WARN("no pr ----");
        return false;
    }

}

void VSTTM::evaluation() {
    vector<RobState> traj_for_one;
    vector<EvalVSTTM>().swap(vec_eval_);
    vector<QE>().swap(vec_QE_);
    qe_sum_ = 0;
    te_sum_ = 0;
    for (double vt = vmin_; vt <= vmax_;) {
        for (double wt = wmin_; wt <= wmax_;) {
            double dis_obs = 0, qe_eval = 0;
            if (!calcQE(vt, wt, qe_eval, dis_obs, traj_for_one)) {
//                out_file_ << "$check," << dis_obs << "," << vt << "," << wt << std::endl;
                wt += WRESL;
                continue;
            }

            double term_cst = CalcTermC(traj_for_one.back());

            vec_eval_.emplace_back(EvalVSTTM(vt, wt, qe_eval, term_cst));
            qe_sum_ += qe_eval;
            te_sum_ += term_cst;

            wt += WRESL;
        }
        vt += VRESL;
    }
}

//生成预估轨迹,轨迹为以(x_circle_, y_circle_)为圆心的曲率圆的圆弧
void VSTTM::genTrajPoint(RobState &p_eval, const double &vt, const double &wt, const double &t) {
    //曲率圆的半径, wt为负时r为负, (x_circle_, y_circle_)为圆心
    double r_circle = vt / wt;
    if (std::isinf(r_circle)) {
        p_eval.pt[0] += vt * t * cos(p_eval.yaw);
        p_eval.pt[1] += vt * t * sin(p_eval.yaw);
    } else {
        double x_circle = p_eval.pt[0] - r_circle * sin(p_eval.yaw);
        double y_circle = p_eval.pt[1] + r_circle * cos(p_eval.yaw);
        //将终点坐标赋予p_eval
        p_eval.pt[0] = x_circle + r_circle * cos(p_eval.yaw - M_PI / 2.0 + wt * t);
        p_eval.pt[1] = y_circle + r_circle * sin(p_eval.yaw - M_PI / 2.0 + wt * t);
        p_eval.yaw += wt * t;
    }

    p_eval.velocity = vt;
    p_eval.angular = wt;
}

bool VSTTM::calcQE(const double &vt, const double &wt, double &qe, double &dis, vector<RobState> &traj) {

    auto ipr = vec_pr_.begin();
    int index_t = 0;
    double dis_min = INT_MAX, dis_tmp;
    double qe_ret = 0, xe, ye;
    for (; ipr != vec_pr_.end(); ++ipr) {
        auto pr = *ipr;

        RobState p_ev(p_->pt, p_->yaw, p_->velocity, p_->angular);
        genTrajPoint(p_ev, vt, wt, index_t * exe_time_);

        map_edt_->getDistance(dis_tmp, p_ev.pt);
        if (dis_tmp <= 2*robot_radius_) {
            std::vector<RobState>().swap(traj);
            return false;
        }
        if (dis_tmp < dis_min)
            dis_min = dis_tmp;

        //calculate the deviation from state vector with reference track point
        xe = cos(p_ev.yaw) * (pr->pt[0] - p_ev.pt[0]) + sin(p_ev.yaw) * (pr->pt[1] - p_ev.pt[1]);
        ye = -sin(p_ev.yaw) * (pr->pt[0] - p_ev.pt[0]) + cos(p_ev.yaw) * (pr->pt[1] - p_ev.pt[1]);
        double thetae = pr->yaw - p_ev.yaw;
        while (thetae > M_PI) {
            thetae -= 2 * M_PI;
        }
        while (thetae < -M_PI) {
            thetae += 2 * M_PI;
        }
        //sum the deviation
        traj.emplace_back(p_ev);
        qe_ret += q1_ * pow(xe, 2) + q2_ * pow(ye, 2) + q3_ * pow(thetae, 2);
        index_t++;
    }

    qe = qe_ret;
    dis = dis_min;
    return true;
}

//calculate the constraint of terminal
double VSTTM::CalcTermC(RobState &state) {
    auto pr = vec_pr_.back();
    double xe = cos(state.yaw) * (pr->pt[0] - state.pt[0]) + sin(state.yaw) * (pr->pt[1] - state.pt[1]);
    double ye = -sin(state.yaw) * (pr->pt[0] - state.pt[0]) + cos(state.yaw) * (pr->pt[1] - state.pt[1]);
    double thetae = pr->yaw - state.yaw;
    while (thetae > M_PI) {
        thetae -= 2 * M_PI;
    }
    while (thetae < -M_PI) {
        thetae += 2 * M_PI;
    }

    vec_QE_.emplace_back(QE(xe, ye, thetae));

    return 0.5 * (pow(xe, 2) + pow(ye, 2) + pow(thetae, 2)) + 1 - cos(thetae);
}

void VSTTM::finalEvaluate() {
    double feval = 0, tempVST;
//    int ki = 0;
    for (int i = 0; i < (int) vec_eval_.size(); ++i) {
        tempVST = vec_eval_[i].qe * e1_
                  + vec_eval_[i].te * e2_;

        if (i == 0) feval = tempVST;

        if (tempVST < feval) {
            feval = tempVST;
            v_output_ = vec_eval_.at(i).v;
            w_output_ = vec_eval_.at(i).w;
        }
    }
}

void VSTTM::normalize() {
    for (int i = 0; i < (int) vec_eval_.size(); ++i) {
        if (qe_sum_ != 0) {
            vec_eval_.at(i).qe /= qe_sum_;
        }
        if (te_sum_ != 0) {
            vec_eval_.at(i).te /= te_sum_;
        }

//        double dp = vec_eval_.at(i)->qe
//                    + vec_eval_.at(i)->te
//                    + vec_eval_.at(i)->de
//                    + vec_eval_[i]->ve;
    }
}

/*** 角度PID控制 --机器人原地自转到预期角度  ***/
/* init_yaw  --rad */
void VSTTM::anglePIDinit(double init_yaw) {
    target_yaw_ = init_yaw;
    has_target_yaw_ = true;
}

void VSTTM::anglePIDCtrl(bool& _finish, double& _vel, double& _anl) {
//    auto& init_yaw = btraj_.init_yaw;
    if(!has_target_yaw_) {
        _finish = true;
        return;
    }

    PidController anglePID(0.5, 0.5, 0, 0, 0.57, -0.57);

    if (target_yaw_ - p_->yaw > M_PI)
        target_yaw_ -= 2 * M_PI;
    else if (target_yaw_ - p_->yaw < -M_PI)
        target_yaw_ += 2 * M_PI;

    if (target_yaw_ - p_->yaw >= (double) M_PI / 24 || target_yaw_ - p_->yaw <= -(double) M_PI / 24) {

        anglePID.pidSetTarget(target_yaw_);
        _vel = 0;
        _anl = anglePID.pidOperation(p_->yaw);

        _finish = false;
    } else {
        _vel = 0;
        _anl = 0;

        _finish = true;
        has_target_yaw_ = false;
    }

}
